/*****************************************************************
*
* ServoDriver.c
* Provides Entry point for Freescale portion of software, and
* manages timer interrupts for the program
*
* Authors
*    Russ Martin (11/7/11)
*    James ODonoghue (11/6/11)
*    Maxwell Macri (11/6/11)
*    Christopher Wood (11/6/11)
*
* Contributors:
*
****************************************************************/

#include <hidef.h>       /* common defines and macros */
#include "Derivative.h"  /* derivative-specific definitions */
#include "LedController.h"
#include "ServoController.h"
 
// Bit Mask for Push Button, Reading on Port B
#define BUTTON_MASK (0x80) 

// LED Status/Error Codes
#define BUTTON_WAIT (0x02)
#define WORKING_OK  (0x01)
#define QNX_FAIL    (0x04)

// Change this value to change the frequency of the output compare signal.
// The value is in Hz.
#define OC_FREQ_HZ    ((UINT16)10)

// Macro definitions for determining the TC1 value for the desired frequency
// in Hz (OC_FREQ_HZ). The formula is:
//
// TC1_VAL = ((Bus Clock Frequency / Prescaler value) / 2) / Desired Freq in Hz
//
// Where:
//        Bus Clock Frequency     = 2 MHz
//        Prescaler Value         = 2 (Effectively giving us a 1 MHz timer)
//        2 --> Since we want to toggle the output at half of the period
//        Desired Frequency in Hz = The value you put in OC_FREQ_HZ
//
#define BUS_CLK_FREQ    ((UINT32)2000000)   
#define PRESCALE        ((UINT16)2)         
#define TC1_VAL         ((UINT16)(((BUS_CLK_FREQ / PRESCALE) / 2) / OC_FREQ_HZ))

// Timer-related constants
#define TIMER1_INT_FLAG (0x02)
#define TIMER2_INT_FLAG (0x04)
#define TIMEOUT_LENGTH  (50)

// Static global variables
static UINT8 timeOutCounter;
static UINT8 endPulseFlag;
static INT8  risingEdgeFlag;

// Initializes I/O and timer settings.
//--------------------------------------------------------------       
void InitializeTimer(void)
{
  // Set the timer prescaler to %2, since the bus clock is at 2 MHz,
  // and we want the timer running at 1 MHz
  TSCR2_PR0 = 1;
  TSCR2_PR1 = 0;
  TSCR2_PR2 = 0;
    
  // Enable output compare on Channel 1
  TIOS_IOS1 = 1;
  
  //Enable input capture on Channel 0
  TIOS_IOS0 = 0;
  
  // Set up output compare action to toggle Port T, bit 1
  TCTL2_OM1 = 0;
  TCTL2_OL1 = 1;
  
  //Set up channel 0 for detecting rising edge
  TCTL4_EDG0B=0;
  TCTL4_EDG0A=1;
  
  // Set up timer compare values
  TC1 = 1000; 
  
  // Clear the Output Compare Interrupt Flag (Channel 1) 
  TFLG1 = TFLG1_C1F_MASK;
  TFLG1 = TFLG1_C0F_MASK;
  
  // Enable the output compare interrupt on Channel 1;
  TIE_C1I = 1;
  TIE_C0I = 1;
  
  //
  // Enable the timer
  // 
  TSCR1_TEN = 1;
   
  //
  // Enable interrupts via macro provided by hidef.h
  //
  EnableInterrupts;

}

// Initializes Port A to have all 8 bits be input (data from QNX)
//-------------------------------------------------------------- 
void InitializePortA( void )
{
  DDRA = 0x00;
}

// The main entry point for the program.
//-------------------------------------------------------------- 
void main(void) 
{
  // Initializations of variables
  UINT8 portAInput;
  
  // Initialize the global variables
  timeOutCounter = 0;
  endPulseFlag = 0;
  risingEdgeFlag = 0;
  
  // Init LED and set the intial state
  InitializeLED();
  DisplayLED(BUTTON_WAIT);
  
  // Wait until push button is pressed down
  while(PORTB & BUTTON_MASK) // Active low, wait for 0
  {
    // Purposely blank
  }
  
  // Output LED to Working State
  DisplayLED(WORKING_OK);
  
  // Init PWM, Timer, and PORT A 
  InitalizePWM();
  timeOutCounter=0;
  InitializeTimer();
  InitializePortA();
  
  // Run the protocol forever
  for(;;) 
  {
    // Send a Pulse
    PORTB_BIT6 = 1;
    
    // Start a timer, end after 1ms
    endPulseFlag=0;
    while(endPulseFlag == 0)
    {
      // Purposely blank
    }
    PORTB_BIT6=0;
    
    // On rising edge, data is ready
    while(risingEdgeFlag == 0 && timeOutCounter < TIMEOUT_LENGTH)
    {
      // Intentionally blank
    }
    
    // Timeout is calculated by TIMEOUT_LENGTH number of timer 1 interrupts
    // Occuring in a row without a proper rising edge interrupt
    // If this happens, go into QNX fail mode, meaning QNX failed/timeout occured
    if(timeOutCounter >= TIMEOUT_LENGTH)
    {
      timeOutCounter=0;
      DisplayLED(QNX_FAIL);
    }
    
    // Rising Edge detected without interrupt
    else if(risingEdgeFlag == 1)
    {
      // Read in the data from QNX
      portAInput = PORTA; 
        
      // Set the PWM signal and LED state
      SetPosition(portAInput);
      DisplayLED(WORKING_OK);
        
      // Reset the rising edge flag
      risingEdgeFlag = 0;
    }
  } 
}

// Output Compare Channel 1 Interrupt Service Routine
// Increments Timeout Counter, and resets timer
//          
// The first CODE_SEG pragma is needed to ensure that the ISR
// is placed in non-banked memory. The following CODE_SEG
// pragma returns to the default scheme. This is neccessary
// when non-ISR code follows. 
//
// The TRAP_PROC tells the compiler to implement an
// interrupt funcion. Alternitively, one could use
// the __interrupt keyword instead.
// 
// The following line must be added to the Project.prm
// file in order for this ISR to be placed in the correct
// location:
//        VECTOR ADDRESS 0xFFEC OC1_isr 
#pragma push
#pragma CODE_SEG __SHORT_SEG NON_BANKED
//--------------------------------------------------------------       
void interrupt 9 OC1_isr( void )
{    
  TCNT = 0;
  
  // Increment the timeout counter and set end pulse flag
  timeOutCounter++;
  endPulseFlag = 1;
  
  // Clear the interrupt flag
  TFLG1 = TFLG1_C1F_MASK;
}

// Input Capture Channel 0 ISR
// Resets Timeout Counter and sets flag
// Notifying that a rising edge has occured
// location:
//        VECTOR ADDRESS 0xFFEE IC0_isr
void interrupt 8 IC0_isr(void)
{
  timeOutCounter = 0;
  risingEdgeFlag = 1;
  
  // Clear the interrupt flag
  TFLG1 = TFLG1_C0F_MASK;
}
#pragma pop

